/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-09-17 17:49:39
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-09-25 22:30:00
 */

#include "RobotCtrl.hpp"



RobotCtrl::RobotCtrl()
{
}

RobotCtrl::RobotCtrl(HardwareSerial &Handle):RobotServoFw(Handle)
{
}

#if !(defined(ESP8266) || defined(ESP32))
RobotCtrl::RobotCtrl(SoftwareSerial &Handle):RobotServoFw(Handle)
{
}
#endif

RobotCtrl::~RobotCtrl()
{
}


/**
 * @function: RobotReset
 * @brief: 机器人复位
 * @param {*}
 * @return {*}
 */
void RobotCtrl::RobotReset(void)
{
    uint16_t Reset_1 = 500;
    uint16_t Reset_2 = 200;
    uint16_t Reset_time = 500;
    RobotSerialServoMove(1, Reset_1, Reset_time);
    RobotSerialServoMove(3, Reset_1, Reset_time);
    RobotSerialServoMove(5, Reset_1, Reset_time);
    RobotSerialServoMove(7, Reset_1, Reset_time);
    RobotSerialServoMove(2, Reset_2, Reset_time);
    RobotSerialServoMove(4, Reset_2, Reset_time);
    RobotSerialServoMove(6, Reset_2, Reset_time);
    RobotSerialServoMove(8, Reset_2, Reset_time);
}


/**
 * @function: Robot_DollyMoves
 * @brief: 机器人前后移动
 * @param {int16_t} offset：移动偏移值，小于0后退，大于0前进
 * @return {*}
 */
void RobotCtrl::Robot_DollyMoves(int16_t offset)
{
    RobotSerialServoMove(2, ROBOT_UP_ANGLE, 50);
    RobotSerialServoMove(6, ROBOT_UP_ANGLE, 50);
    ROBOT_DELAY_MS(50);
    RobotSerialServoMove(1, 500, 100);
    RobotSerialServoMove(5, 500, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(3, ROBOT_SERVO_MIDDLE + (int16_t)(offset * 0.5), 100);
    RobotSerialServoMove(7, ROBOT_SERVO_MIDDLE - (int16_t)(offset * 0.5), 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(2, 200, 100);
    RobotSerialServoMove(6, 200, 100);
    ROBOT_DELAY_MS(100);

    RobotSerialServoMove(4, ROBOT_UP_ANGLE, 50);
    RobotSerialServoMove(8, ROBOT_UP_ANGLE, 50);
    ROBOT_DELAY_MS(50);
    RobotSerialServoMove(3, 500, 100);
    RobotSerialServoMove(7, 500, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(1, ROBOT_SERVO_MIDDLE + offset, 100);
    RobotSerialServoMove(5, ROBOT_SERVO_MIDDLE - offset, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(4, 200, 100);
    RobotSerialServoMove(8, 200, 100);
    ROBOT_DELAY_MS(100);

    RobotSerialServoMove(2, ROBOT_UP_ANGLE, 50);
    RobotSerialServoMove(6, ROBOT_UP_ANGLE, 50);
    ROBOT_DELAY_MS(50);
    RobotSerialServoMove(1, 500, 100);
    RobotSerialServoMove(5, 500, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(3, ROBOT_SERVO_MIDDLE + (int16_t)(offset * 0.3), 100);
    RobotSerialServoMove(7, ROBOT_SERVO_MIDDLE - (int16_t)(offset * 0.3), 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(2, 200, 100);
    RobotSerialServoMove(6, 200, 100);
    ROBOT_DELAY_MS(100);

    RobotSerialServoMove(4, ROBOT_UP_ANGLE, 50);
    RobotSerialServoMove(8, ROBOT_UP_ANGLE, 50);
    ROBOT_DELAY_MS(50);
    RobotSerialServoMove(3, 500, 100);
    RobotSerialServoMove(7, 500, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(4, 200, 100);
    RobotSerialServoMove(8, 200, 100);
    ROBOT_DELAY_MS(100);
}


/**
 * @function: Robot_TuruACorner
 * @brief: 机器人转向函数
 * @param {int16_t} offset：转向偏移值，小于0左转，大于0右转
 * @return {*}
 */
void RobotCtrl::Robot_TuruACorner(int16_t offset)
{
    RobotSerialServoMove(2, ROBOT_UP_ANGLE, 50);
    RobotSerialServoMove(6, ROBOT_UP_ANGLE, 50);
    ROBOT_DELAY_MS(50);
    RobotSerialServoMove(1, ROBOT_SERVO_MIDDLE - offset, 100);
    RobotSerialServoMove(5, ROBOT_SERVO_MIDDLE - offset, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(3, ROBOT_SERVO_MIDDLE + offset, 100);
    RobotSerialServoMove(7, ROBOT_SERVO_MIDDLE + offset, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(2, 200, 100);
    RobotSerialServoMove(6, 200, 100);
    ROBOT_DELAY_MS(100);

    RobotSerialServoMove(4, ROBOT_UP_ANGLE, 50);
    RobotSerialServoMove(8, ROBOT_UP_ANGLE, 50);
    ROBOT_DELAY_MS(50);
    RobotSerialServoMove(3, 500, 100);
    RobotSerialServoMove(7, 500, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(1, 500, 100);
    RobotSerialServoMove(5, 500, 100);
    ROBOT_DELAY_MS(100);
    RobotSerialServoMove(4, 200, 100);
    RobotSerialServoMove(8, 200, 100);
    ROBOT_DELAY_MS(100);
}




